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The  adaptive,  fixed  interval  smoother  uses  raw  cartesian  data  input,  x(t,), 
y(t. ) ,  z( t^ )  for  t.  =  tp  t^,  -  -  -,  tf,  where  t^  is  the  first  measurement 
time  and  t^  is  the  final  trajectory  measurement  time,  to  produce  smooth 
estimates  of  the  trajectory  states,  position,  velocity,  and  acceleration. 

In  contrast  to  other  smoothers  the  fixed  interval  smoother  uses  the  measure¬ 
ments  at  all  trajectory  measurement  times  to  estimate  the  trajectory  states 
at  each  measurement  time.  Thus,  in  a  sense  one  could  say  that  the  smoothing 
interval  is  the  total  trajectory  time.  The  fixed  interval  smoother  is  based 
on  the  modified  Bryson-Frazier  formulation,  denoted  by  mBF,  which  was  develop¬ 
ed  by  Bierman  in  [  1  j.  The  mBF  development  of  the  fixed  interval  smoother 
combines  a  forward  running  Kalman  filter  with  a  backward  adjoint  filter  which 
uses  the  Kalman  filter  residuals  as  input  and  also  uses  other  Kalman  filter 
computed  quantities.  The  mBF  smoother  is  a  very  stable,  computationally 
efficient  form  of  the  fixed  interval  smoother. 

The  fixed  interval  smoother  is  made  adaptive  by  adapting  the  forward  Kalman 
filter  to  the  local  noise  content  of  the  raw  position  data  and  also  adapting 
the  filter  to  acceleration  changes  which  are  sensed  in  the  future  measurements. 
The  Kalman  filter  is  initialized  with  least  squares  state  estimates  at  the 
desired  start  time  and  again  after  any  large  time  break  in  the  measurement 
sequence.  The  smoothing  program  expects  that  the  cartesian  measurement  input 
has  been  preprocessed  to  the  extent  that  it  is  free  from  wild  observations. 

The  smoother  outputs  trajectory  position,  velocity,  and  acceleration  and  esti¬ 
mates  of  the  errors  associated  with  these  quantities.  The  trajectory  states 
and  their  error  estimates  can  be  further  processed  by  an  output  routine  to 
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rotate  and  translate  the  states  to  a  desired  coordinate  systerr  and  origin,  to 
compute  quantities  derivable  from  cartesian  position,  velocity,  and  accelera¬ 
tion,  to  combine  the  trajectory  states  with  atmospheric  measurements,  and  to 
reformat  the  output. 


TRAJECTORY  AND  fxASUREf*E<lT  MODELS 


Let  s(tk)  be  a  3-vector  which  represents  any  of  the  three  coordinates  of  the 
trajectory,  i  ,e.,  s  ■  (  x,  x,  x  ),  or  s  »  (  y,  y,  y  ),  or  s  ■  (  z,  z,  z  ). 
Assume  that  each  coordinate  of  the  trajectory  obeys  the  discrete  time  dynamic 
state  equation 


s(tk  +  ^  »  <t(Ak)  s(tk)  +  y(Ak)u,  (1) 

where  \  "  tk  +  -  tfc.  4>( Ak)  is  the  second  order  transition  matrix 


(2) 


u  is  an  unknown,  constant  scalar  forcing  function  and  y(Ak)  is  the  vector 


*T<v 


(3) 


Let  m(t.)  denote  the  position  measurement  available  to  the  smoother,  m(t^)  is 
represented  as 

mUj)  «  Hs ( ti )  +  e^),  (4) 


where  H  «  [100]  and  eU^)  is  a  zero  mean  measurement  error  with  variance  R(t^) 


KALf'AN  FILTER 


Let  s(k/k)  and  s(k/k-l)  denote  the  filtered  and  predicted  state  estates  at 
time  tk<  Also,  let  P(k/k)  and  P(k/k-l)  denote  the  covariance  matrices  of  the 
filtered  and  predicted  state  estimates  at  time  tR.  The  state  estimates  and 
their  covariances  are  determined  by  the  usual  Kalman,  fil ter  equations, 


s(k+l/k)  =  *(Ak)£(k/k)  (5) 
P(k+l/k)  =  4(Ak)P(k/k)$TUk)  +  q(k)Y(^k)vT(Ak)  (C) 
P( k+l/k+1 )  =  P(k+l/k)  -  P(k+l/k)HT(HP(k+l/k)HT  +  R( tR+1 ) )“ !HP( k+l/k)  (7) 
s(  k+l/k+1)  =  s  (k+-l/k)  +  P(k+l/k+l)HVl(tk+1)(m(tk+1)  -  Hs(k+l/k))  (8) 


The  filter  equations  given  in  (5)  -  (8)  are  not  implemented  directly,  but  are 
implemented  in  a  square  root  form.  Let  the  covariance  matrix  P( k/k )  be  repre¬ 
sented  as  P(k/k)  *  U(k/k)D(k/k)UT(k/k)  where  U(k/k)  is  unit  upper  triangular 
and  D( k/k)  is  diagonal  with  positive  diagonal  elements.  Also,  P(k+l/k)  » 
U(k+l/k)D(k+l/k)UT(k+l/k) .  The  measurement  noise  variance,  R(tk),  1n  the  fil¬ 
ter  equations  represents  the  noise  variance  in  the  vicinity  of  time  tR.  q(k) 
is  a  scalar -representing  the  uncertainty  of  the  unknown  forcing  term,  u. 

At  a  time  update  the  upper  triangular  factor  U  and  diagonal  factor  C  of  the 
predicted  covariance  matrix  are  updated  rather  than  computing  an  updated 
covariance,  P(k+l/k).  The  updated  U-C  factors  U(k+l/k)  and  D(k+l/k)  are 
obtained  via  the  Agee  -  Turner  matrix  factorization  algorithm  given  in 
Appendix  A  and  described  by  Bierman  in  [2],  Thus,  after  a  time  update, 
we  have  the  predicted  state  estimate  computed  from  (5)  and  the  U-D  factors 
U(k+l/k)  and  D(k+l/k)  such  that  P(k+l/k)  -  U(k+l/k)  D(k+l/k)  UT(k+l/k).  If 
U(k/k)  and  D(k/k)  are  the  U-D  factors  of  P(k/k),  then  the  U-D  factors  of  the 
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product  4>(£(,)r(k/k)$  (£k)  in  (6)  are  (since  $>(Ak)  is  upper  triangular), 

l*  *  $(Ak)U(k/k)  (9) 

and 

D  =  D( k/k)  (10) 

The  U-D  factors  of  P(k+l/k)  are  obtained  via  the  algorithm  of  Appendix  A  using 
the  U-D  factors  given  in  (9)  and  (10). 

At  a  measurement  update  the  state  estimate  s(k+l/k+l)  is  computed  from  (S). 

The  U-D  factors,  U(k+l/k+l)  and  D(k+l/k+l),  of  P(k+l/k+l)  are  computed  by  noting 
that  P(k+l/k+l)  given  by  (7)  is  the  sum  of  a  positive  definite  matrix  P(k+l/k), 
whose  U-D  factors  are  available,  and  a  symmetric  dyad  c*uuT  where 
c  *  -(HP(k+l/k)HT  +  rt( t^) ) “ 1  is  a  scalar  and  U  -  P(k+l/k)HT  is  a  3-vector.  The 
algorithm  of  Appendix  A  could  be  used  to  compute  U(k+l/k+l)  and  D(k+l/k+l). 
However,  the  negative  value  of  c  presents  some  potential  problems  in  using  this 
algorithm  so  that  we  will  use  the  algorithm  of  Appendix  B  to  obtain  the  U-D 
factors  of  P(k+l/k+l),  The  algorithm  of  Appendix  B  exploits  the  special  struc¬ 
ture  of  c*uu^  to  compute  the  updated  U-D  factors  more  accurately  than  the  algor¬ 
ithm  of  Appendix  A. 

INITIALIZATION  OF  THE  KALHAN  FILTER 

At  the  beginning  of  the  measurement  sequence  and  at  the  end  of  large  time 
breaks  in  the  measurement  sequence  initial  values  of  the  position,  velocity, 
and  acceleration  states  and  the  U-D  factors  of  their  covariance  matrix  must  be 
supplied  to  the  Kalman  filter.  The  quantities  required  for  initialization  of 
the  Kalman  filter  are  obtained  by  least  squares  estimation  over  the  first  h's 
data  points.  Thus,  if  tj  is  the  time  of  initialization,  the  estimate  s(l/l) 


4' 


used  to  initialize  the  filter  is  obtained  by  minimizing. 


N 

^  (r?C ti )  -  Ho(t.-ti)s(tj))2  (n) 

with  respect  to  s(tj).  The  least  squares  estimate  is  given  by, 

N 

s(l/l)  *  P  ZS^>( t.-t,  )HTn(t. ) ,  (12) 

i*l  1  1  1 


where  P  is  given  by, 

r • 

'S  T 

P  -  I  <p(  t.-t,  )H  H$(  t  .-t1 )  (13) 

i=l  1  1  11 

The  covariance  matrix  of  £{1/1)  is  calculated  from, 

P(l/1)  *  pS2, 

where  a2  is  obtained  from  the  least  squares  residuals  as, 

N. 

S2  «  it^T  Z  («(t,)  -  H4>(  t .  -t,  )s( 1/1 ) ) 2  (14) 

V3  i-1  1  11 

The  U~D  factors  of  P(l/1)  are  obtained  via  the  Cholesky  decomposition  algorithm 
given  in  Appendix  C. 


At  the  present  time  Ns  is  target  dependent  as  in  Table  1.  In  Table  1  T  is  the 
sampling  period  of  the  measurements. 


TABLE  1 


TARGET  CATE  CORY 

Aircraft  -  no  maneuver 
Aircraft  -  maneuver 
Ground  Target 

Low  Altitude  -  no  maneuver 
Low  Altitude  -  maneuver 
He! icopter 

Low  Acceleration  missile 
High  Acceleration  missile 

Also,  N  ^  50. 

MEASUREMENT  NOISE  COVARIANCE 


max  (20,1.5/T) 
max  (20,1 /T) 
max  (20,2/T) 
max  (20,2/T) 
max  (20,1/T) 
max  (20,1.5/T) 
max  (20,1.5/T) 
max  (20,1/T) 


The  measurement  noise  variance  R(t^)  used  in  the  Kalman  filter  is  computed 
from  past  predicted  filter  residuals.  Let  s(k-p+2/k-p+l ) ,  p  »  1,P  be  the  P 
predicted  states  preceding  t^,  P,(tK)  is  computed  from 

p 

R(tK)  I  (m(t,  2)  -  Hs (k-p+2/k-p+l ) ) 2  (16) 

p=l  H 

Presently,  P  *  20. 

STATE  NOISE  COVARIANCE 


The  state  noise  covariance  for  the  Kalman  filter  should  measure  the  uncertainty 
about  the  assumption  of  constant  acceleration.  Thus,  it  is  realistic  to  choose 
/ ~q  to  measure  the  rate  of  change  of  acceleration.  We  could  do  this  by  differ¬ 
encing  the  filtered  acceleration,  SjU/k).  However,  to  obtain  a  quicker  re- 
spn'  e,  we  have  chosen  to  estimate  a  smoothed  acceleration,  s^  (k)  and  differ¬ 
ence  this  quantity.  Let  s^(k)  be  a  3-vector  of  position,  velocity,  and 


acceleration  and  be  the  value  of  s(tk)  which  minimizes 

_  .  _  , 

(s(tu)-s(k/k))TP-1  (k/k)(s(t1/)-s(k/k))+R“1  Z  (m(t^.)-H$(tk+i-tk)s(tk)),  (17) 


k  ^"'^k+i 


where  Rk  is  an  average  measurement  noise  variance  over  the  interval  (tk,tk+I,q) . 
Minimizing  (17)  with  respect  to  s(tk)  results  in  the  smoother  equation 


sq(k)=s(k/k)+Pq(k)R"1  Z  iT(tk+rtk)HT(m(tk+i)-H4»(tk+i-tk)s(k/k))  (18) 

In  (12)  Pq(k)  is  the  covariance  matrix  of  sq(k). 


?q  (k)=P‘1(k/k)+Rk1  Z^T(tk+.-tk)HTH4»(tk+.-tk)  (19) 

The  computation  of  "Rk  is  described  in  Appendix  D. 

The  rate  of  change  of  acceleration  used  in  estimating  q  is  computed  by  differ¬ 
encing  the  smoothed  acceleration  sS(k).  Thus,  we  compute 


a(k)  = 


sq(k)-sq(k-l) 


Wl 


Rather  than  use  a(k)  =  /  q(k),  it  is  much  better  to  smooth  the  sequence,  a ( k ) , 
with  a  fading  memory  filter  and  use  the  smooth  filter  output  to  compute  /  q(k). 
Thus,  we  estimate  a(k)  to  minimize 


k-2  , 

Z  vf(a(k-1)-a(k))2 


where  0<w£l .  The  resulting  steady  state  filter  equation  for  a(k),  obtained  by 
minimizing  (21)  and  then  finding  the  limiting  steady  state  result  is 


(22) 


a(k+l)  =  a(k)  +  (l-l.')(a(k+l  )-a(k)) 

Then  /  q(k)  =  a(k) . 

The  weight  used  in  (22)  is  target  dependent.  The  weights  for  various  categor¬ 
ies  of  targets  are  given  in  Table  2.  The  values  of  W  in  this  table  have  not 
been  optimized  in  any  way  but  have  yielded  good  performance  on  data  from  the 
various  target  types.  These  values  may  be  updated  on  the  basis  of  future  exper¬ 
ience.  An  absolute  upper  limit,  ,  which  is  also  target  dependent,  has  been 
placed  on  /  c;(  k) ,  This  limit,  while  seldom  reached,  is  necessary  to  prevent 
the  Kalman  filter  from  responding  violently  to  occasional  bad  data  (not  spikes) 
situations.  The  values  of  Qu  are  also  given  in  Table  2.  The  values  of  W  given 
table  2  are  for  a  sample  rate  of  20/sec.  The  values  of  W  to  be  used  for  other 
sample  rates  are  calculated  in  the  program  from  the  table  values  in  the  follow¬ 
ing  way.  Let  Un  be  the  table  value  (20/s  value)  for  a  given  target  type  and 
•  % 

let  Vi  be  the  value  to  be  used  at  the  actual  sampling  period,  T.  Then 

log10W  =  1o910;‘*’r  (23) 


TADLE  2 

1-W  . 

,  Qu  , 

TARGET  CATEGORY 

X 

y 

z 

X 

y 

z 

Aircraft  -  no  maneuver 

.05 

.05 

.05 

50 

50 

50 

Aircraft  -  maneuver 

.13 

.13 

.13 

500 

500 

500 

Ground  Target 

.05 

,C5 

.05 

20 

20 

20 

Low  Altitude  -  no  maneuver 

.05 

.05 

.05 

50 

50 

50 

Low  Altitude  -  maneuver 

.13 

.13 

.05 

200 

200 

50 

Helicopter 

.05 

.05 

.05 

50 

50 

50 

Low  Acceleration  missile 

.13 

.13 

.13 

200 

200 

200 

High  Acceleration  missile 

.25 

.25 

.25 

5000 

5000 

5000 

Zm 


ma  x 


Present!/,  the  look  interval  for  state  noise  adaption  is  given  by  N  = 

(23,1.5/T),  where  T  is  sampling  interval  of  the  measurements .  “.Iso,  K  £5C. 

THC  ACJCIKT  FILTEH 

The  adjoint  filter  is  a  Kalman  like  filter  running  backwards  in  time  which  uses 
the  Kalman  filter  residuals  as  input  and  also  uses  the  Kalman  filter  gain  vector. 
The  optimal  fixed  interval  smoother  estimates  are  obtained  by  properly  combining 
the  Kalman  filter  states  with  the  adjoint  filter  states.  Let  A(k-l/k)  and 
A(k/k)  be  the  predicted  and  filtered  state  vectors  (3-vectors)  of  the  adjoint 
filter.  Also,  let  A(k-l/k)  and  A(k/k)  be  the  covariance  matrices  of  the  pre¬ 
dicted  and  filtered  adjoint  state  vectors.  The  equations  governing  the  adjoint 


state  vectors  and  their  covariances  are  given  by: 

A(k/k+l )  *  J(Ak)A(k+l/k+l),  (24) 

A(k/k+l )  =  *T(Ak)A(k+l/k+l  )$(£,,),  (25) 

A( k/k)  =  A(k/k+l )  -  H^Vu/k-lJ+D^U/k+l)),  (26) 

A(k/k)  =  (I-KkH)TA(k/k+l)(I-KkH)+HTD*1H.  (27) 

In  the  above  equations  r(k/k-l)  is  the  predicted  Kalman  filter  residual, 
r(k/k-l)  ■  m(tk)-Hs(k/k-l),  (28) 

Dk  is  the  covariance  of  this  predicted  residual, 

Dk  *  MP(k/k-l)HT+n(tk),  (29) 

and  Kj,is  the  Kalman  filter  gain  vector, 

Kk  -  P(k/k-l)HTD‘1  (30) 


In  contrast  to  the  Kalman  filter  the  equations  of  the  adjoint  filter  are  imple¬ 
mented  directly  rather  than  using  a  matrix  square  root  implementation. 


Q 

* 


The  adjoint  filter  is  initialized  at  the  final  trajectory  time,  t,.,  and  at 
the  end  of  any  measurement  sequence  preceeding  a  large  time  break.  The  ad¬ 
joint  filter  is  initialized  by. 


X(f,/N)  =  -HTD;,1r(N/N-l) 

A(N/K)  =  H^r.Vl 

•  1 


(31) 

(32) 


CPTI.VAL  SMOOTHED  ECTI FTATES 


The  optimal,  fixed  interval  smoothed  estimates  are  obtained  by  combining  the 
states  of  the  Kalman  filter  with  the  states  of  the  adjoint  filter.  Assuming 
trajectory  state  estimates  are  desired  only  at  the  measurement  times,  the 
smoothed  estimates  are  computed  from, 

s(k)  =  s ( k/k)  -  P(k/k) A(k/k+l )  (33) 

and  the  covariance  of  this  estimate  can  be  computed  from  (if  desired  for  out¬ 
put), 

P(k)  =  P( k/k)  -  P ( k/k) A( k/k+1 )P(k/k)  (34) 
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The  matrix  factorization  algorithm  described  below  for  obtaining  the  'J-D  fac¬ 
tors  of  a  positive  definite  matrix  plus  a  symmetric  dyad  was  first  reported  in 
[3]  and  later  described  by  Bierman  in  [  2  j . 

A 

Given  a  positive  definite  matrix  P,  a  vector  V,  and  a  scalar  c,  we  form  the 
matrix  ?, 

~  T 

P  ■  P  +  cVV 

A 

Suppose  we  have  a  unit  upper  triangular  matrix  l'  and  a  positive  diagonal 

A  a  A  Ay 

matrix  D  such  that  P  *  UDU  .  An  algorithm  for  computing  the  U-C  factors, 
u”  and  d",  of?  is  given  by  the  following  sequence  of  steps, 

D(I)  i  8(1)  +  c V2 ( I )  I-K.l 

V(K)  •  V(K)  -  V(l)  U(K,1)  ] 

^  -  CV(I )  K*1,I*' 

U(K.I)  =  U(K,I)  +  - -  V(K) 

ctn  ) 

.  CD(I) 

w  . 

In  the  above  algorithm  the  symbol  =  represents  the  FORTRAN  replacement 
equality.  Note  that  the  computation  of  the  U-D  factors  can  be  done  in 
place  with  the  C(I)  stored  along  the  diagonal  of  U. 


APPENDIX  S 


The  following  algorithm  is  taken  from  3ierman  [2],  p77,  This  algorithm  com¬ 
putes  the  t-D  factors  of  the  posterior  Kalman  filter  covariance  matrix, 

P( k+l/k+1 )  =  P. 

P  =  ?  -  *PHT(HPHT  +  R)“  lHP”  (Dl) 

Let  U  and  D  be  the  L'-D  factors  of  P, 

P  =  UCUT  (B2) 

Let  f,  v,  and  g^  be  !i-vectors 

~T  T 

f  =  U*H 1 


v  *  Df 


a,  «  v(l)f(l)  +  R,  gj  =  (v(l)O— 0) 


3(1)  ■  DdJR/o, 

aJ  *  aJ-l  +  v( 

D(J)  =  DCJjaj^/aj 

U(K,J)  *  U(K,J)  -  f(J)gJ(K)/aJ_1 

gJ+l (K)  «  3j(K)  +  V(J)U(K,J) 


K-1,J 


J*2,N 


The  vector  g,^/at,  will  be  the  Kalman  filter  gain.  The  computation  of  U  and  D 
can  be  done  in  place,  if  desired. 
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APPENDIX  C 


The  algorithm  presented  below  obtains  the  U-D  factors  of  the  positive  de¬ 
finite,  NXN  matrix  P,  P  *  'JCu\  where  U  is  upper  triangular  and  D  is  a  pos¬ 
itive  diagonal  matrix. 


0(K)  «  ?(N.N) 


U(K/K)  =  P(K,N)/D(N),  K*1 ,  N-l 


t; 

D(I)  *  P(I,I)  -  I  Ul(I,K)0(K),  I-N-1,1 
K*I+1 


It 

U(K.I)  -  (P(K.I)  -  Z  U(K,L)D(L)U(I,L))/D(I),K«1,  1-1,  I«N-1,2 
L*K+1 


APPENDIX  C 


The  average  noise  variance,  R^,  over  the  interval  (tk,  tk+f,^)  is  computed  by 
fitting  a  quadratic  curve  to  the  measurements  in  the  interval  and  then  letting 
R„  be  the  variance  of  the  measurement  residuals  from  the  fit.  Consider  the 
quadratic  model , 


»(Vi>  ' a  *  i,[tuftk>  *  c(tk*rVs*  S 

Let  a,  b,  c  be  the  least  squares  estimates  of  a,  b,  c.  The  residuals  from 
this  fit  are 

6(k+i)  «  ra(tk+i)  -  a  -  b  (tk+i-tk)  -  c  (tk+i-tk)2,  1»1,  Nq 
The  noise  variance  R.,  used  is  computed  by 


Hq 

Z  o2(k+i ) 
1-1 


This  value  is  used  only  if  Nq>10.  If  Nq<10,  RK  x  R C tk)  is  used. 
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